#include "CtrlMsgHandler.h"
#include "SimDriverBoard.h"
#include "DriverModule/DriverManager.h"
#include "DriverModule/ConcreteDriver/Forker.h"
#include "DriverModule/ConcreteDriver/Motor.h"
#include "DriverModule/ConcreteDriver/IO.h"

const static std::string MSG_CTRL_TYPE = "ctrl";        // 消息类型
const static int MSG_CTRL_LENGTH = 50;                  // 消息长度

CtrlMsgHandler::CtrlMsgHandler()
{
    SetMsgInfo(MSG_CTRL_TYPE, MSG_CTRL_LENGTH);
    // 清空数据对象
    m_ctrl.clear();
}

bool CtrlMsgHandler::Init()
{
    // 获取驱动对象，方便在处理协议的时候，直接使用对象。【注意：这里就涉及到SimDriverBoard各个服务模块的先后注册顺序问题】
    SimDriverBoard& simDriverBoard = SimDriverBoard::Instance();
    m_driverManager = simDriverBoard.GetManager<DriverManager>();
    if (m_driverManager == nullptr) 
    {
        std::cout << "CtrlMsgHandler::Init() error   m_driverManager == nullptr" << std::endl;
        return false;
    }

    m_forker = m_driverManager->GetDriver<Forker>("Forker");    // 货叉控制
    if (m_forker == nullptr)
    {
        return false;
    }

    m_motor1 = m_driverManager->GetDriver<Motor>("motor1");     // 电机控制
    m_motor2 = m_driverManager->GetDriver<Motor>("motor2");
    m_chargeIO = m_driverManager->GetDriver<IO>("charge");      // 充电开关

    return true;
}

bool CtrlMsgHandler::HandleMsgSub(const std::string& msg)
{
    m_ctrl = msg;
    
    CtrlEnable();
    MotorCtrl();
    LightCtrl();
    SetForkHeight();
    
    return true;
}



void CtrlMsgHandler::CtrlEnable()
{
    // TODO: 使能控制
    m_chargeIO->SetIO(true);        // 充电使能
}

void CtrlMsgHandler::LightCtrl()
{
    //  TODO: 灯光控制
}


void CtrlMsgHandler::SetForkHeight()
{
    if (m_forker != nullptr && m_forker->IsEnable())
    {
        // 从m_ctrl解析出目标高度
        // int targetHeight = parseForkHeight(m_ctrl);
        // m_forker->SetTargetHeight(targetHeight);
    }
}

// 电机控制
void CtrlMsgHandler::MotorCtrl()
{
    if (m_motor1 != nullptr)
    {
        m_motor1->SetTargetSpeed(/*parseSpeed(m_ctrl)*/ 0);
    }
    if (m_motor2 != nullptr)
    {
        m_motor2->SetTargetSpeed(/*parseSpeed(m_ctrl)*/ 0);
    }
}
